/* Includes ------------------------------------------------------------------*/
#include "global.h"
#include <stdio.h>
#include <string.h>
#include "test_kf.h"

/* global variable ---------------------------------------------*/
struct event_queue event_queue = {};

struct vehicle_instance g_drone;

/* private function ---------------------------------------------*/
void global_var_init(void);
void imu_data_handler(struct imu_raw_data *sensor_raw);
void rcinput_handler(struct rc_raw_input *rc_raw);
void loop_start(void);
void loop_end(void);

/*
 * @brief  Main program
 * @param  None
 * @retval None
 */
int main(void)
{
	uint8_t type = 0;
	void *event_payload = 0;

	board_init();
	global_var_init();
	printf("fc start\r\n");

	/* Infinite loop */
	while (1) {
		/* auxiliary things to do before loop*/
		loop_start();

		type = event_fetch(&event_queue, &event_payload);

		if (type == EVENT_NONE) {
			/* count the tick, check if something fails */
		} else if (type == EVENT_IMU_DATA) {
			struct imu_raw_data imu_raw =
				*((struct imu_raw_data*)event_payload);
			imu_data_handler(&imu_raw);
		} else if (type == EVENT_RC_INPUT) {
			struct rc_raw_input rc_raw =
				*((struct rc_raw_input *)event_payload);
			rcinput_handler(&rc_raw);
		} else if (type == EVENT_MAG_DATA) {
			struct mag_raw_data mag_raw =
				*((struct mag_raw_data *)event_payload);
#if 0
			printf("%d %d %d\r\n", mag_raw.mag[0],
			       mag_raw.mag[1],
			       mag_raw.mag[2]);
#endif
		}
		/* auxiliary things to do after loop*/
		loop_end();
	}
}

void imu_data_handler(struct imu_raw_data *sensor_raw)
{
#if 0
	/* set g_drone.pose */

	estimate_attitude(sensor_raw);

	/*functions below are driven by fastest event */

	/* set g_drone.actuator_ctrl */

	rate_control();

	/* set g_drone.mixer_output */

	output_mixing();

	/* write to pwm timer */

	pwm_out();
#endif
}

void rcinput_handler(struct rc_raw_input *rc_raw)
{
#if 0
	/* check if rc calibrated (set flag and warn if not?) */

	rc_check_calibrated(rc_raw);

	/* update vehicle status */
	printf("rc input: %d %d %d %d %d\r\n" , rc_raw->channels[0]
	       , rc_raw->channels[1]
	       , rc_raw->channels[2]
	       , rc_raw->channels[3]
	       , rc_raw->timestamp);
#endif
}

void global_var_init()
{
	state_init(&g_drone.status);

	param_load_all();
	/* init g_drone maybe */
}

void loop_start(void)
{
}

void loop_end(void)
{
	static uint64_t start_time = 0;
	uint64_t now = 0;
	now = board_get_systime();
	if (start_time == 0)
		start_time = now;
	if (now - start_time > 2e4) {
		start_time = now;
		board_led_toggle(0);
		board_led_toggle(1);
		board_led_toggle(2);
		board_led_toggle(3);
		run_kalman_test();
	}
}

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
